package com.mydomain;


import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;
import lejos.nxt.SoundSensor;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.DifferentialPilot;

/***
 *ClassName: MazePrototypeV3 
 *Description: The main class of the program contains the variables to be used by all the diffrerent threads
 *				Using Multi Threading algorithm, to allow the NXT to follow the fall on its left
 *				Difference from v2- Use manual motor control
 */
public class CopyOfMazePrototypeV3 {
	CopyOfMazePrototypeV3 mpv3;
	boolean tooNearToWall;
	boolean tooFarFromWall;
	boolean noWallsSurroundings;
	public boolean isNoWallsSurroundings() {
		return noWallsSurroundings;
	}

	public void setNoWallsSurroundings(boolean noWallsSurroundings) {
		this.noWallsSurroundings = noWallsSurroundings;
	}

	public boolean isTooNearToWall() {
		return tooNearToWall;
	}

	public void setTooNearToWall(boolean tooNearToWall) {
		this.tooNearToWall = tooNearToWall;
	}

	public boolean isTooFarFromWall() {
		return tooFarFromWall;
	}

	public void setTooFarFromWall(boolean tooFarFromWall) {
		this.tooFarFromWall = tooFarFromWall;
	}

	boolean touched=false;
	
	public boolean isTouched() {
		return touched;
	}

	public void setTouched(boolean touched) {
		this.touched = touched;
	}

	public static void main(String args[])
	{
		while (!Button.LEFT.isPressed()) {
			LCD.drawString("Press LEFT", 0, 0);
			LCD.drawString("to start: ", 0, 1);
			LCD.refresh();
		}
		CopyOfMazePrototypeV3 mpv3= new CopyOfMazePrototypeV3();
		ManeuverNXT mnxt=new ManeuverNXT(mpv3);
		UltrasonicReading ur1= new UltrasonicReading(mpv3);
		TouchSensorReading tr1= new TouchSensorReading(mpv3);
		tr1.start();
		ur1.start();
		mnxt.start();
		while (!Button.ESCAPE.isPressed()) {
			// Empty
		}
		
	}
}
/***
 * ClassName: ManeuverNXT
 *  Description: Controls the different motors in the NXT 
 *  			Allows the NXT to turn properly when a object is infront
 */
class ManeuverNXT extends Thread {
	CopyOfMazePrototypeV3 mnxt;
	final int FORWARD = 1;
	final int STOP = 3;
	final int FLT = 4;
	final int POWER = 80;
	DifferentialPilot pilot;

	public ManeuverNXT(CopyOfMazePrototypeV3 inmnxt) {
		mnxt = inmnxt;
	}

	public void run() {
		while (mnxt.touched == false) {
			pilot = new DifferentialPilot(2.1f, 4.4f, Motor.A, Motor.B, false);
			pilot.setTravelSpeed(3);
			pilot.forward();
		}
		while (true) {
			// set the speed for the different motors
			pilot = new DifferentialPilot(2.1f, 4.4f, Motor.A, Motor.B, false);
			pilot.setTravelSpeed(5);
			Motor.A.setSpeed(5);
			Motor.B.setSpeed(5);
			Motor.C.setSpeed(5);
				if (mnxt.isTouched() == false) {
					if ((mnxt.isTooFarFromWall() == false)
							&& (mnxt.isTooNearToWall() == true)) {
						pilot.travel(0.5);
						pilot.rotate(10);
					} else if ((mnxt.isTooFarFromWall() == true)
							&& (mnxt.isTooNearToWall() == false)) {
						pilot.travel(0.5);
						pilot.rotate(-10);
					} else {
						pilot.forward();
					}
				} else {
					pilot.travel(-3);
					pilot.rotate(30);
					pilot.stop();
				}
			}
		}
	}
/***
 * ClassName: UltrasonicReading
 *  Description:Controls the ultransonic reading and the position of the sensor
 */
class UltrasonicReading extends Thread {
	int tooNearToWall = 15;
	int tooFarFromWall = 40;

	
	UltrasonicSensor us1 = new UltrasonicSensor(SensorPort.S3);
	CopyOfMazePrototypeV3 mnxt;
	int readValue;

	public UltrasonicReading(CopyOfMazePrototypeV3 inmnxt) {
		mnxt = inmnxt;
	}

	public void run() {
		while (true) {
			/**
			 * if the distance between the nxt and the wall is less than 10 turn it to the right
			 * 	if the distance between the nxt and the wall is more than 35 turn it to theleft and walk towards the wall
			 * else the Nxt will move forward
			 * */
			
			readValue = us1.getDistance();
			LCD.clear();
			LCD.drawInt(readValue, 0, 5);
			 if (readValue <= tooNearToWall) {
				mnxt.setTooFarFromWall(false);
				mnxt.setTooNearToWall(true);
			} else if (readValue >= tooFarFromWall) {
				mnxt.setTooFarFromWall(true);
				mnxt.setTooNearToWall(false);
			} else {
				mnxt.setTooFarFromWall(false);
				mnxt.setTooNearToWall(false);
			}
			try {
				Thread.sleep(2000);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
	}
}
/***
 * ClassName: TouchSensorReading
 *  Description:Detects if the touch sensor of the NXT is being pressed
 */
class TouchSensorReading extends Thread {
	TouchSensor touch = new TouchSensor(SensorPort.S1);
	CopyOfMazePrototypeV3 mnxt;
	public TouchSensorReading(CopyOfMazePrototypeV3 inmnxt)
	{
		mnxt=inmnxt;
	}
	public void run()
	{
		while(true)
		{
			if(touch.isPressed())
			{
				mnxt.setTouched(true);
				
			}else{
				mnxt.setTouched(false);
			}
		}
	}
	
}